﻿
using AForge.Imaging;
using System;
using System.Collections;
using System.Collections.Generic;
using System.Drawing;
using System.Drawing.Imaging;
using System.Linq;

namespace ELTE.TFEG.ImageTransform.Hough
{


    public class HoughTransform
    {
        #region line-search data
        private int Line_StepsPerDegree;
        private int Line_HoughHeight { get; set; }
        private double Line_thetaStep;

        private double[] sinCache;
        private double[] cosCache;

        private int[,] HoughMap { get; set; }

        private List<HoughLine> lines = new List<HoughLine>();

        public int StepsPerDegree
        {
            get { return Line_StepsPerDegree; }
            set
            {
                Line_StepsPerDegree = value < 1 ? 1 : (value > 10 ? 10 : value);
                //stepsPerDegree = Math.Max(1, Math.Min(10, value));
                Line_HoughHeight = 180 * Line_StepsPerDegree;
                Line_thetaStep = Math.PI / Line_HoughHeight;

                // precalculate Sine and Cosine values
                sinCache = new double[Line_HoughHeight];
                cosCache = new double[Line_HoughHeight];

                for (int i = 0; i < Line_HoughHeight; i++)
                {
                    sinCache[i] = Math.Sin(i * Line_thetaStep);
                    cosCache[i] = Math.Cos(i * Line_thetaStep);
                }
            }
        }

        private int MinLineIntensity { get; set; }

        private int LocalPeakRadius { get; set; }

        public int MaxIntensity { get; private set; }


        public int LinesCount
        {
            get { return lines.Count; }
        }
        #endregion

        #region circle-search data
        // circle radius to detect
        public int CircleRadiusToDetect { get; set; }

        // Hough map
        private int[,] Circle_HoughMap { get; set; }
        private int Circle_MaxMapIntensity = 100;

        // Hough map's width and height
        private int Circle_Width { get; set; }
        private int Circle_Height { get; set; }

        private int Circle_LocalPeakRadius = 4;
        private int Circle_MinCircleIntensity = 10;
        private List<HoughCircle> Circles = new List<HoughCircle>();

        public int MinCircleIntensity
        {
            get { return Circle_MinCircleIntensity; }
            set { Circle_MinCircleIntensity = value; }
        }
        public int CircleLocalPeakRadius
        {
            get { return Circle_LocalPeakRadius; }
            set { Circle_LocalPeakRadius = Math.Max(1, Math.Min(10, value)); }
        }
        public int CircleMaxIntensity
        {
            get { return Circle_MaxMapIntensity; }
            set { Circle_MaxMapIntensity = value; }
        }
        public int CirclesCount
        {
            get { return Circles.Count; }
        }


        #endregion

        public HoughTransform()
        {
            StepsPerDegree = 1;
            MinLineIntensity = 10;
            LocalPeakRadius = 4;
            MaxIntensity = 0;

            CircleLocalPeakRadius = 15;
        }

        #region line-search methods
        public void Line_Generate(Bitmap image)
        {
            Line_Generate(image, new Rectangle(0, 0, image.Width, image.Height));
        }


        public void Line_Generate(Bitmap image, Rectangle rect)
        {
            // check image format
            if (image.PixelFormat != PixelFormat.Format8bppIndexed)
            {
                throw new UnsupportedImageFormatException("Unsupported pixel format of the source image.");
            }

            // lock source image
            BitmapData imageData = image.LockBits(
                new Rectangle(0, 0, image.Width, image.Height),
                ImageLockMode.ReadOnly, PixelFormat.Format8bppIndexed);

            try
            {
                // process the image
                Line_Generate(new UnmanagedImage(imageData), rect);
            }
            finally
            {
                // unlock image
                image.UnlockBits(imageData);
            }
        }

        public void Line_Generate(BitmapData imageData)
        {
            Line_Generate(new UnmanagedImage(imageData),
                new Rectangle(0, 0, imageData.Width, imageData.Height));
        }

        public void Line_Generate(BitmapData imageData, Rectangle rect)
        {
            Line_Generate(new UnmanagedImage(imageData), rect);
        }

        public void Line_Generate(UnmanagedImage image)
        {
            Line_Generate(image, new Rectangle(0, 0, image.Width, image.Height));
        }

        public void Line_Generate(UnmanagedImage image, Rectangle rect)
        {
            if (image.PixelFormat != PixelFormat.Format8bppIndexed)
            {
                throw new UnsupportedImageFormatException("Unsupported pixel format of the source image.");
            }

            // get source image size
            int width = image.Width;
            int height = image.Height;
            int halfWidth = width / 2;
            int halfHeight = height / 2;

            // make sure the specified rectangle recides with the source image
            rect.Intersect(new Rectangle(0, 0, width, height));

            int startX = halfWidth.Negate() + rect.Left;
            int startY = halfHeight.Negate() + rect.Top;
            int stopX = width - halfWidth - (width - rect.Right);
            int stopY = height - halfHeight - (height - rect.Bottom);

            int offset = image.Stride - rect.Width;

            // calculate Hough map's width
            int halfHoughWidth = (int)Math.Sqrt(halfWidth * halfWidth + halfHeight * halfHeight);
            int houghWidth = halfHoughWidth * 2;

            HoughMap = new int[Line_HoughHeight, houghWidth];

            // do the job
            unsafe
            {
                byte* src = (byte*)image.ImageData.ToPointer() +
                    rect.Top * image.Stride + rect.Left;

                // for each row
                for (int y = startY; y < stopY; y++)
                {
                    // for each pixel
                    for (int x = startX; x < stopX; x++, src++)
                    {
                        if (*src != 0)
                        {
                            // for each Theta value
                            for (int theta = 0; theta < Line_HoughHeight; theta++)
                            {
                                int radius = (int)Math.Round(cosCache[theta] * x - sinCache[theta] * y) + halfHoughWidth;

                                if ((radius < 0) || (radius >= houghWidth))
                                {
                                    continue;
                                }
                                else
                                {
                                    HoughMap[theta, radius]++;
                                }
                            }
                        }
                    }
                    src += offset;
                }
            }

            // find max value in Hough map
            MaxIntensity = 0;
            for (int i = 0; i < Line_HoughHeight; i++)
            {
                for (int j = 0; j < houghWidth; j++)
                {
                    if (HoughMap[i, j] > MaxIntensity)
                    {
                        MaxIntensity = HoughMap[i, j];
                    }
                }
            }

            CollectLines();
        }

        public Bitmap Line_GetHoughMap(Bitmap image)
        {
            // check if Hough transformation was made already
            if (HoughMap == null)
            {
                Line_Generate(image);
            }

            int width = HoughMap.GetLength(1);
            int height = HoughMap.GetLength(0);

            // create new, graycaled image
            Bitmap img = AForge.Imaging.Image.CreateGrayscaleImage(width, height);

            //img = img.GetNonIndexedBitmap();
            // lock destination bitmap data
            BitmapData imageData = img.LockBits(
                new Rectangle(0, 0, width, height),
                ImageLockMode.ReadWrite, PixelFormat.Format8bppIndexed);

            int offset = imageData.Stride - width;
            float scale = 255.0f / MaxIntensity;

            // do the job
            unsafe
            {
                byte* dst = (byte*)imageData.Scan0.ToPointer();

                for (int y = 0; y < height; y++)
                {
                    for (int x = 0; x < width; x++, dst++)
                    {
                        *dst = (byte)System.Math.Min(255, (int)(scale * HoughMap[y, x]));
                    }
                    dst += offset;
                }
            }



            // unlock destination images
            img.UnlockBits(imageData);

            return img;
        }

        public HoughLine[] GetMostIntensiveLines(int count)
        {
            // lines count
            int n = Math.Min(count, lines.Count);

            // result array
            //TODO: LINQ
            HoughLine[] dst = new HoughLine[n];
            lines.CopyTo(0, dst, 0, n);

            return dst;
        }

        public HoughLine[] GetLinesByRelativeIntensity(double minRelativeIntensity)
        {

            int count = lines.Count(line => line.RelativeIntensity >= minRelativeIntensity);

            return GetMostIntensiveLines(count);
        }


        // Collect lines with intensity greater or equal then specified
        private void CollectLines()
        {
            int maxTheta = HoughMap.GetLength(0);
            int maxRadius = HoughMap.GetLength(1);

            int intensity;
            bool foundLocalMaximum = false;

            int halfHoughWidth = maxRadius >> 1;

            // clean lines collection
            lines.Clear();

            // for each Theta value
            for (int theta = 0; theta < maxTheta; theta++)
            {
                // for each Radius value
                for (int radius = 0; radius < maxRadius; radius++)
                {
                    // get current value
                    intensity = HoughMap[theta, radius];

                    if (intensity < MinLineIntensity)
                        continue;

                    foundLocalMaximum = false;

                    // check neighbours
                    for (int tt = theta - LocalPeakRadius, ttMax = theta + LocalPeakRadius; tt < ttMax; tt++)
                    {
                        //if it is not the local maximum we should break
                        if (foundLocalMaximum.Equals(true))
                        {
                            break;
                        }

                        int cycledTheta = tt;
                        int cycledRadius = radius;

                        // check limits
                        if (cycledTheta < 0)
                        {
                            cycledTheta = maxTheta + cycledTheta;
                            cycledRadius = maxRadius - cycledRadius;
                        }
                        if (cycledTheta >= maxTheta)
                        {
                            cycledTheta -= maxTheta;
                            cycledRadius = maxRadius - cycledRadius;
                        }

                        for (int tr = cycledRadius - LocalPeakRadius, trMax = cycledRadius + LocalPeakRadius; tr < trMax; tr++)
                        {
                            // skip out of map values
                            if (tr < 0)
                                continue;
                            if (tr >= maxRadius)
                                break;

                            // compare to the neighbour with the current value
                            if (HoughMap[cycledTheta, tr] > intensity)
                            {
                                foundLocalMaximum = true;
                                break;
                            }
                        }
                    }

                    // was it local maximum ?
                    if (!foundLocalMaximum)
                    {
                        // we have local maximum
                        lines.Add(new HoughLine
                        {
                            ThetaAngle = Convert.ToDouble((double)theta / Line_StepsPerDegree),
                            Radius = radius - halfHoughWidth,
                            Intensity = intensity,
                            RelativeIntensity = Convert.ToDouble((double)intensity / MaxIntensity)
                        });
                    }
                }
            }

            lines.Sort();
        }
        #endregion

        #region circle-search methods
        public void Circle_Generate(Bitmap image, int radius)
        {
            this.CircleRadiusToDetect = radius;
            // check image format
            if (image.PixelFormat != PixelFormat.Format8bppIndexed)
            {
                throw new UnsupportedImageFormatException("Unsupported pixel format of the source image.");
            }

            // lock source image
            BitmapData imageData = image.LockBits(
                new Rectangle(0, 0, image.Width, image.Height),
                ImageLockMode.ReadOnly, PixelFormat.Format8bppIndexed);

            try
            {
                // process the image
                Circle_Generate(new UnmanagedImage(imageData));
            }
            finally
            {
                // unlock image
                image.UnlockBits(imageData);
            }
        }

        public void Circle_Generate(BitmapData imageData)
        {
            Circle_Generate(new UnmanagedImage(imageData));
        }

        public void Circle_Generate(UnmanagedImage image)
        {
            if (image.PixelFormat != PixelFormat.Format8bppIndexed)
            {
                throw new UnsupportedImageFormatException("Unsupported pixel format of the source image.");
            }

            Circles.Clear();

            // get source image size
            this.Circle_Width = image.Width;
            this.Circle_Height = image.Height;

            int srcOffset = image.Stride - Circle_Width;

            // allocate Hough map of the same size like image
            this.Circle_HoughMap = new int[Circle_Height, Circle_Width];

            // do the job
            unsafe
            {
                byte* src = (byte*)image.ImageData.ToPointer();

                // for each row
                for (int y = 0; y < Circle_Height; y++)
                {
                    // for each pixel
                    for (int x = 0; x < Circle_Width; x++, src++)
                    {
                        if (*src != 0)
                        {
                            DrawHoughCircle(x, y);
                        }
                    }
                    src += srcOffset;
                }
            }

            // find max value in Hough map
            this.CircleMaxIntensity = 0;
            for (int i = 0; i < this.Circle_Height; i++)
            {
                for (int j = 0; j < this.Circle_Width; j++)
                {
                    if (Circle_HoughMap[i, j] > CircleMaxIntensity)
                    {
                        CircleMaxIntensity = Circle_HoughMap[i, j];
                    }
                }
            }

            CollectCircles();
        }
        public Bitmap ToBitmap()
        {
            // check if Hough transformation was made already
            if (this.Circle_HoughMap == null)
            {
                throw new ApplicationException("Hough transformation was not done yet.");
            }

            int width = Circle_HoughMap.GetLength(1);
            int height = Circle_HoughMap.GetLength(0);

            // create new image
            Bitmap image = AForge.Imaging.Image.CreateGrayscaleImage(width, height);

            // lock destination bitmap data
            BitmapData imageData = image.LockBits(
                new Rectangle(0, 0, width, height),
                ImageLockMode.ReadWrite, PixelFormat.Format8bppIndexed);

            int offset = imageData.Stride - width;
            float scale = 255.0f / this.CircleMaxIntensity;

            // do the job
            unsafe
            {
                byte* dst = (byte*)imageData.Scan0.ToPointer();

                for (int y = 0; y < height; y++)
                {
                    for (int x = 0; x < width; x++, dst++)
                    {
                        *dst = (byte)System.Math.Min(255, (int)(scale * Circle_HoughMap[y, x]));
                    }
                    dst += offset;
                }
            }

            // unlock destination images
            image.UnlockBits(imageData);

            return image;
        }
        public HoughCircle[] GetMostIntensiveCircles(int count)
        {
            // lines count
            int n = Math.Min(count, Circles.Count);

            // result array
            HoughCircle[] dst = new HoughCircle[n];
            Circles.CopyTo(0, dst, 0, n);

            return dst;
        }
        public HoughCircle[] GetCirclesByRelativeIntensity(double minRelativeIntensity)
        {

            int count = Circles.Count(c => c.RelativeIntensity >= minRelativeIntensity);
            return GetMostIntensiveCircles(count);
        }

        private void CollectCircles()
        {
            int intensity;
            bool foundGreater;

            // clean circles collection
            Circles.Clear();

            // for each Y coordinate
            for (int y = this.CircleRadiusToDetect; y < this.Circle_Height - this.CircleRadiusToDetect; y++)
            {
                // for each X coordinate
                for (int x = this.CircleRadiusToDetect; x < this.Circle_Width - this.CircleRadiusToDetect; x++)
                {
                    // get current value
                    intensity = this.Circle_HoughMap[y, x];

                    if (intensity < this.Circle_MinCircleIntensity)
                        continue;

                    foundGreater = false;

                    // check neighboors
                    for (int ty = y - this.CircleLocalPeakRadius, tyMax = y + CircleLocalPeakRadius; ty < tyMax; ty++)
                    {
                        // continue if the coordinate is out of map
                        if (ty < 0)
                            continue;
                        // break if it is not local maximum or coordinate is out of map
                        if ((foundGreater == true) || (ty >= Circle_Height))
                            break;

                        for (int tx = x - CircleLocalPeakRadius, txMax = x + CircleLocalPeakRadius; tx < txMax; tx++)
                        {
                            // continue or break if the coordinate is out of map
                            if (tx < 0)
                                continue;
                            if (tx >= Circle_Width)
                                break;

                            // compare the neighboor with current value
                            if (this.Circle_HoughMap[ty, tx] > intensity)
                            {
                                foundGreater = true;
                                break;
                            }
                        }
                    }

                    // was it local maximum ?
                    if (!foundGreater)
                    {
                        // we have local maximum
                        double calcIntensity = (double)intensity / (double)this.CircleMaxIntensity;
                        Circles.Add(
                            new HoughCircle
                            {
                                Center = new Point(x, y),
                                Radius = this.CircleRadiusToDetect,
                                Intensity = intensity,
                                RelativeIntensity = calcIntensity
                            }
                            );
                    }
                }
            }

            //Circles.Sort();
            Circles.Sort();

            //this.Circles = DistinctCloseElements(this.Circles);
        }

        private void DrawHoughCircle(int xCenter, int yCenter)
        {
            int x = 0;
            int y = CircleRadiusToDetect;
            int p = (5 - CircleRadiusToDetect * 4) / 4;

            SetHoughCirclePoints(xCenter, yCenter, x, y);

            while (x < y)
            {
                x++;
                if (p < 0)
                {
                    p += 2 * x + 1;
                }
                else
                {
                    y--;
                    p += 2 * (x - y) + 1;
                }
                SetHoughCirclePoints(xCenter, yCenter, x, y);
            }
        }

        private void SetHoughCirclePoints(int cx, int cy, int x, int y)
        {
            if (x == 0)
            {
                SetHoughPoint(cx, cy + y);
                SetHoughPoint(cx, cy - y);
                SetHoughPoint(cx + y, cy);
                SetHoughPoint(cx - y, cy);
            }
            else if (x == y)
            {
                SetHoughPoint(cx + x, cy + y);
                SetHoughPoint(cx - x, cy + y);
                SetHoughPoint(cx + x, cy - y);
                SetHoughPoint(cx - x, cy - y);
            }
            else if (x < y)
            {
                SetHoughPoint(cx + x, cy + y);
                SetHoughPoint(cx - x, cy + y);
                SetHoughPoint(cx + x, cy - y);
                SetHoughPoint(cx - x, cy - y);
                SetHoughPoint(cx + y, cy + x);
                SetHoughPoint(cx - y, cy + x);
                SetHoughPoint(cx + y, cy - x);
                SetHoughPoint(cx - y, cy - x);
            }
        }

        // Set point
        private void SetHoughPoint(int x, int y)
        {
            if (
                (x >= CircleRadiusToDetect && x < this.Circle_Width - CircleRadiusToDetect)
                && (y >= CircleRadiusToDetect && y < this.Circle_Height - CircleRadiusToDetect)
                )
            {
                this.Circle_HoughMap[y, x]++;
            }
        }

        private Point GetAverage(List<HoughCircle> circles)
        {
            Point p;

            int maxX = circles.Max(m => m.Center.X);
            int minX = circles.Min(m => m.Center.X);
            int maxY = circles.Max(m => m.Center.Y);
            int minY = circles.Min(m => m.Center.Y);

            p = new Point(minX, minY).Average(new Point(maxX, maxY));

            return p;
        }

        public List<HoughCircle> DistinctCloseElements(List<HoughCircle> circles)
        {
            try
            {
                //remove the circles where the circles are intersects each other
                List<HoughCircle> circlesToDistinct = new List<HoughCircle>();
                List<HoughCircle> circlesDistincted = new List<HoughCircle>();

                for (int i = 0; i < circles.Count; i++)
                {
                    circlesToDistinct.Clear();
                    circlesToDistinct = circles.Where(c => c.IntersectsOrContains(circles[i])).ToList();
                    circlesToDistinct.Add(circles[i]);
                    //the closest to the average of the center points will be the one we will keep
                    Point average = GetAverage(circlesToDistinct);
                    HoughCircle avgCircle = new HoughCircle
                        {
                            Center = average,
                            Radius = this.CircleRadiusToDetect,
                            Intensity = Convert.ToInt32(circlesToDistinct.Average(c => c.Intensity)),
                            RelativeIntensity = circlesToDistinct.Average(c => c.RelativeIntensity)
                        };
                    if (!circlesDistincted.Any(c => c.Center == avgCircle.Center))
                    {
                        circlesDistincted.Add(
                            avgCircle
                            );
                    }
                    circles.RemoveAll(c => circlesToDistinct.Contains(c));

                }
                //circles.AddRange(circlesDistincted);
            }
            catch (Exception ex)
            {

            }
            return circles;
        }
        #endregion

        public Bitmap GetOriginalImageWithLinesFound(Bitmap originalImage, double minIntensity)
        {
            Line_Generate(originalImage);
            Hough.HoughLine[] lines = GetLinesByRelativeIntensity(minIntensity);
            List<Hough.HoughLine> l = lines.ToList();
            Bitmap bmpNonIndexed = originalImage.GetNonIndexedBitmap();
            Pen pen = new Pen(Color.Red, 1.5F);
            l.ForEach(
                    line =>
                    {
                        bmpNonIndexed = line.TryDrawOnImage(bmpNonIndexed, pen);
                    });
            //HoughLineTransformation tr = new HoughLineTransformation();
            //tr.ProcessImage(originalImage);
            //var linesBy = tr.GetLinesByRelativeIntensity(minIntensity);
            //linesBy.ToList().ForEach(li => {
            //    HoughLine lin = new HoughLine { Intensity = li.Intensity, Radius = li.Radius, RelativeIntensity = li.RelativeIntensity, ThetaAngle = li.Theta };
            //    lin.TryDrawOnImage(bmpNonIndexed, pen);
            //});
            
            
            return bmpNonIndexed;
        }

        public Bitmap GetOriginalImageWithLineSegmentsFound(Bitmap originalImage, double minIntensity)
        {
            Line_Generate(originalImage);
            Hough.HoughLine[] lines = GetLinesByRelativeIntensity(minIntensity);
            List<Hough.HoughLine> l = lines.ToList();
            Bitmap bmpNonIndexed = originalImage.GetNonIndexedBitmap();
            Pen pen = new Pen(Color.GreenYellow, 1.5F);
            l.ForEach(
                    line =>
                    {
                        bmpNonIndexed = line.TryDrawLineSegment(bmpNonIndexed, pen);
                    });
            return bmpNonIndexed;
        }

        public Bitmap GetOriginalImageWithCirclesFound(Bitmap originalImage, double minIntensity, int circleRadius, bool distinct, bool traceDown)
        {
            List<HoughCircle> circlesFound = new List<HoughCircle>();
            if (traceDown)
            {
                for (int i = circleRadius - 10; i < circleRadius; i++)
                {
                    Circle_Generate(originalImage, i);
                    //Circle_Generate(originalImage, circleRadius);
                    if (distinct)
                    {
                        circlesFound.AddRange(DistinctCloseElements(GetCirclesByRelativeIntensity(minIntensity).ToList()));
                    }
                    else
                    {
                        circlesFound.AddRange(GetCirclesByRelativeIntensity(minIntensity));
                    }
                    
                }
            }
            else
            {
                Circle_Generate(originalImage, circleRadius);
                circlesFound.AddRange(GetCirclesByRelativeIntensity(minIntensity));
                if (distinct)
                {
                    circlesFound = DistinctCloseElements(circlesFound);
                }
            }
            
            var minInt = circlesFound.Min(m => m.Intensity);
            var maxInt = circlesFound.Max(m => m.Intensity);

            var avgIntensity = (minInt + maxInt) / 2;

            circlesFound = circlesFound.Where(c => c.Intensity > avgIntensity ).ToList();
            //HoughCircleTransformation transform = new HoughCircleTransformation(circleRadius);
            //transform.ProcessImage(originalImage);
            //var circles = transform.GetCirclesByRelativeIntensity(minIntensity);
            //circles.ToList().ForEach(c =>
            //    circlesFound.Add(
            //        new HoughCircle
            //        {
            //            Center = new Point { X = c.X, Y = c.Y },
            //            Intensity = c.Intensity,
            //            Radius = c.Radius,
            //            RelativeIntensity = c.RelativeIntensity
            //        }));

            Bitmap bmpNonIndexed = originalImage.GetNonIndexedBitmap();
            //Pen pen = new Pen(Color.Orange, 1.5F);
            Pen pen = new Pen(Color.Orange, 1.0F);
            circlesFound.ForEach(circle =>
                {
                    bmpNonIndexed = circle.TryDrawOnImage(bmpNonIndexed, pen);
                });
            return bmpNonIndexed;
        }
    }
}
